from typing import Tuple, List, Sequence
import time
import numpy as np
import cv2
import os
import json

def decode_json(data):
    # 尝试解析JSON字符串，失败则返回None
    try:
        parsed_data = json.loads(data)
        return parsed_data
    except json.JSONDecodeError:
        print("接收到的数据不是有效的 JSON")
        return None

"""
读取保存为 JSON 格式的配置文件，并返回一个字典。
输入:
    file (str): 配置文件的路径。
输出：
    dict: 包含配置信息的字典。
"""
def load_config(file):
    """
    读取保存为JSON格式的配置文件，并返回一个字典。
    输入:
        file (str): 配置文件的路径。
    输出：
        dict: 包含配置信息的字典。
    """
    # 检查文件是否存在
    if not os.path.isfile(file):
        raise FileNotFoundError(f"配置文件 {file} 不存在.")
    # 读取文件内容
    with open(file, 'r', encoding='utf-8') as f:
        config = json.load(f)
    return config

"""
计算像素坐标系下的点距离原点的绝对距离
注意：这里的坐标系是以摄像机为原点,向右为x正方向,向下为y正方向方向.
输入:
    pixel_x (int): 像素坐标系下的x坐标
    pixel_y (int): 像素坐标系下的y坐标
    pixel_per_cm_ratio (float): 像素与厘米的比值, 1cm = ?pixel
输出:
    tuple (float, float): x方向和y方向上的距离(单位: cm)
"""
def compute_pixel_to_dist(
    pixel_x: int, pixel_y: int, pixel_per_cm_ratio: float
) -> Tuple[float, float]:
    """
    计算像素坐标系下的点距离原点的绝对距离
    注意：这里的坐标系是以摄像机为原点, 向右为x正方向, 向下为y正方向方向。
    输入:
        pixel_x (int): 像素坐标系下的x坐标
        pixel_y (int): 像素坐标系下的y坐标
        pixel_per_cm_ratio (float): 像素与厘米的比值, 1cm = ?pixel
    输出:
        tuple (float, float): x方向和y方向上的距离(单位: cm)
    """
    dist_x, dist_y = pixel_x / pixel_per_cm_ratio, pixel_y / pixel_per_cm_ratio
    return round(dist_x, 2), round(dist_y, 2)

"""
给定起点和终点, 将其连线平均分为若干等份
输入:
    start_point (int, int): 像素坐标下的起始点
    end_point (int, int): 像素坐标下的终点
    num_points (int): 等分的份数
输出:
    list ((int, int)): num_points 个坐标, 每一个坐标用(x, y)表示
"""
def create_line_points(
    start_point: Tuple[int, int], end_point: Tuple[int, int], num_points: int = 6
) -> List[Tuple[int, int]]:
    """
    给定起点和终点, 将其连线平均分为若干等份
    输入:
        start_point (int, int): 像素坐标下的起始点
        end_point (int, int): 像素坐标下的终点
        num_points (int): 等分的份数
    输出:
        list ((int, int)): num_points 个坐标, 每一个坐标用(x, y)表示
    """
    points = []
    x_values = np.linspace(start_point[0], end_point[0], num_points)
    y_values = np.linspace(start_point[1], end_point[1], num_points)
    for x, y in zip(x_values, y_values):
        points.append((int(x), int(y)))
    return points

"""
两个点的欧式距离小于threshold,则认为他们重合，否则不重合.
输入:
    position_a tuple(int, int): 点a
    position_b tuple(int, int): 点b
输出:
    bool: True / False
"""
def is_matched(
    position_a: Tuple[int, int], position_b: Tuple[int, int], threshold: int = 5
) -> bool:
    """
    两个点的欧式距离小于threshold,则认为他们重合，否则不重合.
    输入:
        position_a tuple(int, int): 点a
        position_b tuple(int, int): 点b
    输出:
        bool: True / False
    """
    if (position_a[0] - position_b[0]) ** 2 + (
        position_a[1] - position_b[1]
    ) ** 2 <= threshold**2:
        return True
    return False

"""
判断检测到的Apriltags 是否被放置到了目标位置。当检测有未被放置到目标位置的tags, 返回其id, x, y。如果没有,则返回-1, -1, -1
输入:
    target_positions list(points): 目标位置的列表，每一个元素为(x,y)
    tags list(tuple): 检测到的Apriltags 列表 (id, x, y)
输出:
    Tuple[int, int, int]: 当某个id的apriltag 位置和目标位置列表中对应index元素的坐标不重合时，返回[id, x, y], 如果没有的返回[-1, -1, -1]
"""
def mismatched_position(
    target_positions: List[Tuple[int, int]],
    tags: List[Tuple[int, int, int]],
    threshold: int = 5,
) -> Tuple[int, int, int]:
    for id, tag_x, tag_y in tags:
        res = is_matched(target_positions[id], (tag_x, tag_y), threshold)
        if not res:
            return id, tag_x, tag_y
    return -1, -1, -1

"""
在图像上绘制目标区域
输入:
    image (numpy array): 要绘制的图像
    points (list of tuple): 每个元素是一个二元组 (x, y)，表示一个点的坐标
    width (int): Default=40, 目标方块的边长
    radius (int): Default=4, 圆点的颜色, 默认为红色(BGR)
    alpha (float): default=0.3, 图像透明度
    color (tuple): 圆点的颜色，默认为绿色 (BGR格式)
    thickness (int): 线条的粗细。如果是负数（例如-1），则表示要绘制实心圆。
"""
def draw_target_pos(
    image: np.ndarray,
    points: List[Tuple[int, int]],
    width: int = 40,
    radius: float = 4,
    alpha: float = 0.3,
    color: Tuple[int, int, int] = (0, 255, 0),
    thickness: int = -1,
) -> np.ndarray:
    """
    在图像上绘制目标区域
    输入:
        image (numpy array): 要绘制的图像
        points (list of tuple): 每个元素是一个二维组 (x, y)，表示一个点的坐标
        width (int): Default=40, 目标方块的边长
        radius (int): Default=4, 圆点的颜色, 默认为红色(BGR)
        alpha (float): default=0.3, 图像透明度
        color (tuple): 圆点的颜色，默认为绿色 (BGR格式)
        thickness (int): 线条的粗细。如果是负数（例如-1），则表示要绘制实心圆。
    """
    overlay = image.copy()
    for key, (x, y) in points.items():
        top_left = (int(x - width / 2), int(y - width / 2))
        bottom_right = (int(x + width / 2), int(y + width / 2))
        cv2.rectangle(overlay, top_left, bottom_right, color, -1)
        cv2.circle(overlay, (x, y), radius, (0, 0, 255), thickness)
        cv2.putText(
            overlay,
            f"ID:{key}",
            (top_left[0], top_left[1] - 10),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.5,
            color,
            2,
        )
    cv2.addWeighted(overlay, alpha, image, 1 - alpha, 0, image)
    return image

"""
在图像中画十字
计算十字的四个端点坐标
在图像上画十字
"""
def draw_cross(
    frame: np.ndarray,
    point: Sequence[int],
    size: int = 20,
    color: Tuple[int, int, int] = (0, 0, 255),
    thickness: int = 2,
) -> None:
    """
    在图像中画十字
    计算十字的四个端点坐标
    在图像上画十字
    """
    x, y = point
    p1 = (x - size, y)
    p2 = (x + size, y)
    p3 = (x, y - size)
    p4 = (x, y + size)
    cv2.line(frame, p1, p2, color, thickness)
    cv2.line(frame, p3, p4, color, thickness)

"""
在图像上绘制检测到的apriltag的位置和id。
参数:
    frame (np.ndarray): 图像帧。
    id (int): 检测到的apriltag的id。
    point (Sequence[int]): 检测到的apriltag的位置，作为包含两个整数的序列 (x, y)。
    size (int, 可选): 标签的大小，默认值为20。
    color_cross (Tuple[int, int, int], 可选): 绘制apriltag十字准线的颜色，默认值为 (0, 0, 255) 表示红色。
    color_text (Tuple[int, int, int], 可选): 绘制apriltag的ID，默认值为 (255,255,0)。
    font_shift (Tuple[int, int], 可选): 调整绘制文字的位置偏移。
    thickness (int, 可选): 绘制标签的线条粗细，默认值为2。
返回:
     None
"""
def draw_tag(
    frame: np.ndarray,
    id: int,
    point: Tuple[int, int],
    size: int = 20,
    color_cross: Tuple[int, int, int] = (0, 0, 255),
    color_text: Tuple[int, int, int] = (255, 255, 0),
    font_shift: Tuple[int, int] = (25, 5),
    thickness: int = 2,
) -> None:
    """
    在图像上绘制检测到的apriltag的位置和id。
    参数:
        frame (np.ndarray): 图像帧。
        id (int): 检测到的apriltag的id。
        point (Sequence[int]): 检测到的apriltag的位置，作为包含两个整数的序列 (x, y)。
        size (int, 可选): 标签的大小，默认值为20。
        color_cross (Tuple[int, int, int], 可选): 绘制apriltag十字准线的颜色，默认值为 (0, 0, 255) 表示红色。
        color_text (Tuple[int, int, int], 可选): 绘制apriltag的ID，默认值为 (255,255,0)。
        font_shift (Tuple[int, int], 可选): 调整绘制文字的位置偏移。
        thickness (int, 可选): 绘制标签的线条粗细，默认值为2。
    返回:
         None
    """
    draw_cross(frame, point, size, color_cross, thickness)
    cv2.putText(
        frame,
        f"ID:{id}",
        (point[0] + font_shift[0], point[1] + font_shift[1]),
        cv2.FONT_HERSHEY_SIMPLEX,
        0.8,
        color_text,
        1,
    )

"""
根据输入的step, 判断是否能够执行
"""
def _check_valid(step):
    """
    根据输入的step, 判断是否能够执行
    """
    if step is None:
        return False
    else:
        return True
